#include "SGKAgent.h"
#include "matrix4.h"
#include "vec3.h"
#include "GOD.h"
#include <map>
#include <vector>
#include <queue>
#include "OpenHeap.h"
#include "node.h"

extern HDC hDC;
extern int nFontList;

using std::queue;
using std::map;
using std::vector; // Aww naw.


CBrain::CBrain()
{
	m_pCurrentAction = 0;
}

CBrain::~CBrain()
{
	for (size_t i = 0; i < availableActions.size(); ++i)
		delete availableActions[i];
}

void CBrain::init(SGKAgent * owner) // BOING //
{
	m_pOwner = owner;
	availableActions.push_back((Action *)new ActionFireWeapon(m_pOwner));
	availableActions.push_back((Action *)new ActionMountTurret(m_pOwner));
	availableActions.push_back((Action *)new ActionAimGun(m_pOwner));
	availableActions.push_back((Action *)new ActionMelee(m_pOwner));
	availableActions.push_back((Action *)new ActionGOTO(m_pOwner));
	availableActions.push_back((Action *)new ActionJustChillin(m_pOwner));
	availableActions.push_back((Action *)new ActionGotoPlayer(m_pOwner));
}


//returns the amount of conditions that are not satisfied
float CBrain::disparity(Action * from, Action * to)
{
	float disp = 0.0f;
	for (unsigned int i = 0; i < to->pre.size(); ++i)
	{
		if (God::GetInstance()->EvaluateCondition(m_pOwner, to->pre[i]).b_value != value::zero().b_value)
		{
			disp += 1.0f; // Increment for every false condition.
		}
	}

	return disp;
}

bool isAStarNodeGreaterThan(node const* lhs, node const* rhs)
{
	if (lhs->finalCost > rhs->finalCost) return true;

	return false;
}

bool CBrain::formulatePlan(Condition * goal, value val)
{

	//PlannerNode* aStar(datatype start, datatype goal)
	//{
	//	map created;
	//	priority_queue open;
	//	created[start] = new PlannerNode(start, 0, distance(start, goal));
	//	open.enqueue(created[start]);

	// Let's get a goal action
	while (!m_plan.empty())
		m_plan.pop();

	Condition t_goal = *goal;
	t_goal.active = val;

	Action * a_goal = 0;

	std::map<Action *, node *> created;
	OpenHeap open(isAStarNodeGreaterThan);

	if (!m_pCurrentAction)
	{
		m_pCurrentAction = availableActions[5]; // default to just chillin
	}

	for (unsigned int i = 0; i < availableActions.size(); ++i)
	{
		if (availableActions[i]->satisfies(&t_goal)){
			a_goal = availableActions[i];
			created[a_goal] = new node(a_goal, 0.f, disparity(m_pCurrentAction, a_goal), NULL);
			open.push(created[a_goal]);
		}
	}

	if (!a_goal) return false;

	//
	//	while (!open.empty())
	//	{
	//	    Node* current = open.popfront();
	//	    if this node is the goal, return it;
	//	    generate successors for current;

	vector<node> successors;

	while (!open.empty())
	{
		node * current = open.front();
		open.pop();

		if (current->m_Action && current->m_Action->satisfied()) // if current action is doable...
		{
			// TODO: build solution list and exit!
			while (current->m_Parent)
			{
				m_plan.push((current->m_Action));
				current = current->m_Parent;
			}
			m_plan.push((current->m_Action));

			// now delete our allocated memory:
			for (std::map<Action *, node *>::iterator iter = created.begin(); iter != created.end(); ++iter)
			{
				delete iter->second;
			}

			return true;
		}

		successors.clear();

		for (unsigned int i = 0; i < availableActions.size(); ++i)
		{
			bool good = true;

			for (unsigned int j = 0; j < current->m_Action->pre.size() && good; ++j)
			{
				if (!availableActions[i]->satisfies(&current->m_Action->pre[j]))
				{
					good = false;
					break;// unneccesssarrry.
				}
			}

			if (good)
				successors.push_back(node(availableActions[i], current->finalCost, disparity(availableActions[i], current->m_Action), current));
		}

		//
		//	    for each successor state of current:
		//	    {
		//	        float tempGCost = parent->givenCost + successorDistance * weight;
		//

		for (unsigned int i = 0; i < successors.size(); ++i)
		{
			float tempGCost = successors[i].m_Parent->givenCost + (successors[i].m_Action->animationTime * 1.0f);
			//	        if state is a duplicate,
			//	            if original given cost is greater,


			if (created.find(successors[i].m_Action) != created.end())
			{
				if (created[successors[i].m_Action]->givenCost > tempGCost)
				{
					//	                remove original from open list (if there);
					//	                original->parent = current;

					open.remove(&successors[i]);
					successors[i].m_Parent = current;

					//			original->givenCost = tempGCost;
					//			original->finalCost = givenCost + original->heuristicCost;
					//			open.push(open, original);

					successors[i].givenCost = tempGCost;
					successors[i].finalCost = tempGCost + successors[i].heuristicCost;
					open.push(&successors[i]);
					//
					//	        else if state is not a duplicate,
					//	            heuristicCost = distance(successor, goal);
					//	            created[state] = new PlannerNode(state, tempGCost, heuristicCost);
					//	            open.push(created[state]);

				}
			} else {
				float heuristic = disparity(successors[i].m_Action, a_goal);
				created[successors[i].m_Action] = new node(successors[i].m_Action, tempGCost, heuristic, current); // TODO: Make sure parent pointer is set and stays.
				open.push(created[successors[i].m_Action]);
			}
		}
	}

	return false;

}

void CBrain::getOwned(SGKAgent * owner){m_pOwner = owner;}

void CBrain::update(float dt)
{
	if (m_plan.size() == 0)
	{
		formulatePlan(God::GetInstance()->Conditions(PLAYERHURT), value(true)); // todo: create a master list of actions with enumerateable indices for easy access YAY! [DONE]  later: load these from file.  even later: make a tool.
	}


	if (m_pCurrentAction)
	{
		m_pCurrentAction->animationTime -= dt;

		if (!m_pCurrentAction->satisfies(&Condition(PLAYERHURT, value(true))) && m_pCurrentAction->animationTime < 0.0f)
		{
			// TODO: our post conditions are set to true as a result of our scripted animation system that doesn't exist, and thusly the world reflects the requests of our actions.
			m_plan.pop();
			m_pCurrentAction = 0;
			// THIS IS WHAT YOUR ACTIONS REQUESTED IS IT NOT.
		}
	}

	if (!m_pCurrentAction && m_plan.size())
	{
		m_pCurrentAction = m_plan.front();
		m_pCurrentAction->activate();
	}

}

SGKAgent::SGKAgent()
{
	m_Matrix.make_identity();
	m_Dest.x = 0.0;
	m_Dest.y = 0.0;
	m_Dest.z = 0.0;
	m_pTarget = 0;
	m_pObject = 0;
}

void SGKAgent::Init(void)
{
	quad = gluNewQuadric();
	gluQuadricNormals(quad, true);

	m_Brain.init(this);
	m_pTarget = 0;
	m_pObject = 0;
}

void SGKAgent::Kill(void)
{
	gluDeleteQuadric(quad);
}

void SGKAgent::DrawText(char * text, vec3f pos)
{
	glColor3f(1.0f, 1.0f, 1.0f);
	glRasterPos3f(pos.x, pos.y, pos.z);
	glListBase(nFontList);
	glCallLists((GLsizei)strlen(text), GL_UNSIGNED_BYTE, text);
}


void SGKAgent::Draw(void)
{
	glPushMatrix();
	glColor3f(0.1f, 1.f, 0.1f);

	glMultMatrixf(m_Matrix.ma);

	gluSphere(quad, m_fRadius, 50, 50);
	gluCylinder(quad, (double)m_fRadius * 1.025, 0.0, (double)m_fRadius * 3.14, 24, 24);

	char buffer[256];
	ZeroMemory(buffer, 256);
	//Draw Current Action Name
	DrawText(m_Brain.currentBehavior(), vec3f(0.f, 0.2f, 0.f));

	glPopMatrix();

}

void SGKAgent::Update(float dt)
{
	dt *= God::GetInstance()->GetTimeMod();

	// This call fixes a pointer discrepancy bug caused by the resizing of the Agent vector.  No need for this in a proper implementation.
	m_Brain.getOwned(this);

	if (strcmp(m_Brain.currentBehavior(), "Goto") == 0)
	{
		// OUR FSM HAS TWO STATES, GOTO AND ANIMATE.  The thing above this is old and should be deleted.

		char * test = m_Brain.currentBehavior();

		if (!m_pObject || (m_pObject && m_pObject->m_pOwner != this))
		{
			// enter state
			m_pObject = God::GetInstance()->GetObject(God::GetInstance()->findMeA((OBJID)m_Brain.GetCurrentAction()->pre[0].active.u_value)); // Get a handle to the object we want to go to!

			if (m_pObject)
				m_pObject->m_pOwner = this;
		}

		if (m_pObject)
		{
			m_Dest = m_pObject->pos;
			m_Matrix.zx = (m_Dest.x - m_Matrix.wx);
			m_Matrix.zy = (m_Dest.y - m_Matrix.wy);
			m_Matrix.zz = (m_Dest.z - m_Matrix.wz);

			float len = sqrt((m_Matrix.zx * m_Matrix.zx) + (m_Matrix.zy * m_Matrix.zy) + (m_Matrix.zz * m_Matrix.zz));
			m_Matrix.zx /= len;
			m_Matrix.zy /= len;
			m_Matrix.zz /= len;

			m_Matrix.wx += m_Matrix.zx * dt; // i know.
			m_Matrix.wy += m_Matrix.zy * dt; // * len
			m_Matrix.wz += m_Matrix.zz * dt; // kind of defeats the purpose.  but if this were implemented with a scriptable animation system, we wouldnt have to do stuff like this.

			if (len < 0.1f)
			{
				m_Brain.update(dt); // exit state
			}
		}

	} else {
		// ANIMATE
		m_Brain.update(dt);
	}

	//if (m_pObject && m_pObject->m_pOwner)
	//	m_pObject->pos = vec3f(m_Matrix.wx, m_Matrix.wy, m_Matrix.wz);
}

